// User/fuzzy.hpp 头文件内容
#ifndef FUZZY_HPP
#define FUZZY_HPP

#include "main.hpp"

class FuzzyPID {
private:
    float base_kp = 2;
    float base_ki = 0.1;
    float base_kd = 0.1;

    // enum FuzzySet { NB = 0, NM, NS, ZO, PS, PM, PB };

    // 需要调节的参数区域 ----------------------------
    float error_max = 100.0f;    // 系统最大误差值（根据实际系统量程修改）
    float delta_error_max = 10.0f; // 误差变化率最大值（根据系统响应速度调整）
    float kp_max = 1.5f;         // 比例系数最大调整量（根据系统振荡情况调整）
    float ki_max = 0.3f;         // 积分系数最大调整量（根据稳态误差调整）
    float kd_max = 0.5f;         // 微分系数最大调整量（根据系统阻尼调整）
    
    // 模糊规则表（7x7矩阵对应NB到PB七个模糊集）
// 模糊集定义：NB=负大, NM=负中, NS=负小, ZO=零, PS=正小, PM=正中, PB=正大
float kp_rule[7][7] = {
        {1.0f, 1.0f, 0.6f, 0.6f, 0.3f, 0.0f, 0.0f},   // NB
        {1.0f, 1.0f, 0.6f, 0.3f, 0.3f, 0.0f, -0.3f},  // NM
        {0.6f, 0.6f, 0.6f, 0.3f, 0.0f, -0.3f, -0.3f}, // NS
        {0.6f, 0.3f, 0.3f, 0.0f, -0.3f, -0.3f, -0.6f},// ZO
        {0.3f, 0.3f, 0.0f, -0.3f, -0.3f, -0.6f, -0.6f},// PS
        {0.0f, 0.0f, -0.3f, -0.6f, -0.6f, -0.6f, -1.0f},// PM
        {0.0f, 0.0f, -0.3f, -0.6f, -0.6f, -1.0f, -1.0f} // PB
    };


float ki_rule[7][7] = {
    {-1.0f, -1.0f, -0.6f, -0.6f, -0.3f,  0.0f,  0.0f},  // NB
    {-1.0f, -0.6f, -0.6f, -0.3f,  0.0f,  0.3f,  0.3f},  // NM
    {-0.6f, -0.6f, -0.3f,  0.0f,  0.3f,  0.6f,  0.6f},  // NS
    {-0.3f, -0.3f,  0.0f,  0.3f,  0.6f,  0.6f,  1.0f},   // ZO
    { 0.0f,  0.0f,  0.3f,  0.6f,  0.6f,  1.0f,  1.0f},  // PS
    { 0.3f,  0.3f,  0.6f,  0.6f,  1.0f,  1.0f,  1.0f},  // PM
    { 0.6f,  0.6f,  1.0f,  1.0f,  1.0f,  1.0f,  1.0f}   // PB
};


float kd_rule[7][7] = {
    {0.5f, 0.3f, 0.0f, -0.3f, -0.5f, -0.8f, -1.0f},
    {0.3f, 0.1f, -0.1f, -0.3f, -0.6f, -0.8f, -1.0f},
    {0.1f, -0.1f, -0.3f, -0.5f, -0.6f, -0.8f, -1.0f},
    {-0.1f, -0.3f, -0.5f, -0.6f, -0.8f, -1.0f, -1.0f},
    {-0.3f, -0.5f, -0.6f, -0.8f, -1.0f, -1.0f, -1.0f},
    {-0.5f, -0.6f, -0.8f, -1.0f, -1.0f, -1.0f, -1.0f},
    {-0.8f, -1.0f, -1.0f, -1.0f, -1.0f, -1.0f, -1.0f}
};


    // ---------------------------------------------

    // PID参数
    float kp, ki, kd;   
    float integral = 0.0f;
    float prev_error = 0.0f;

    // 模糊化函数
    float fuzzyfication(float value, float max);

    void calculateMembership(float value, float* membership);
    
    // 解模糊（重心法）
    float defuzzification(float* membership);

public:
    FuzzyPID(float p, float i, float d) : base_kp(p), base_ki(i), base_kd(d),kp(p), ki(i), kd(d) {}

    // 更新控制量（需要循环调用）
    // current_value：当前反馈值
    // target：目标设定值
    float update(float current_value, float target);
};

#endif // FUZZY_HPP
